package uns.fit.gis.alg;
import java.util.Vector;

import uns.fit.gis.Util;
import uns.fit.gis.mapdrawer.LayerConfig;
import uns.fit.gis.mapdrawer.MapCanvas;
import uns.fit.gis.mapobj.Crossroads;
import uns.fit.gis.mapobj.Point;
import uns.fit.gis.mapobj.Segment;

public class RoutingAlgorithm {
	// For Dijkstra algorithm
	private static int[] distanceArray;
	private static short[] prevVertexArray;
	private static boolean[] isVisited;
        private static Queue queue;
	// For graphics
	private int sourceX;
	private int sourceY;
	private int destinationX;
	private int destinationY;
	
	private int projectionSourceX;
	private int projectionSourceY;
	private int sourceClosestSegmentId;
	private int sourceClosestPointIndex;
	private int directionTowardSource;
	private int projectionDestinationX;
	private int projectionDestinationY;
	private int destinationClosestSegmentId;
	private int destinationClosestPointIndex;
	private int directionTowardDestination;
	private Crossroads source;
	private Crossroads destination;
	// For displaying result
	private boolean resultGenerated;
	private Vector crossroadsTrace;
	private Vector segmentTrace;
	public Vector bannedSegmentsIds;
	
	public RoutingAlgorithm() {
		int numVertices = MapCanvas.cross.length;
		
		distanceArray = new int[numVertices];
		prevVertexArray = new short[numVertices];
		isVisited = new boolean[numVertices];
		
		crossroadsTrace = new Vector();
		segmentTrace = new Vector();
		bannedSegmentsIds = new Vector();
		
		directionTowardDestination = directionTowardSource = 0;

                listPoints = new Vector();
	}
	
	/*
	 * Neu co du lieu duong 1 chieu thi ham nay cho ket qua sai
	 */
	private int getAdjacentSegmentId(int cross0, int cross1) {
		for(int i = 0; i < MapCanvas.cross[cross0].getSegments().length; i++)
			for(int j = 0; j < MapCanvas.cross[cross1].getSegments().length; j++)
				if(MapCanvas.cross[cross0].getSegments()[i] == MapCanvas.cross[cross1].getSegments()[j])
					return MapCanvas.cross[cross0].getSegments()[i];
		return -1;
	}
	/*
	 * Ham cai tien
	 * Chon segment co 2 dau la cross0 va cross1
	 * Tuy thuoc vao tu cross0 co toi cross1 hay khong de tra ket qua
	 */
	public static int getAdjacentSegmentId2(int cross0, int cross1) {
		for(int i = 0; i < MapCanvas.cross[cross0].getSegments().length; i++) {
			if(MapCanvas.segments[MapCanvas.cross[cross0].getSegments()[i]].getCross0() == cross1 ||
					MapCanvas.segments[MapCanvas.cross[cross0].getSegments()[i]].getCross1() == cross1)
				return MapCanvas.cross[cross0].getSegments()[i];
		}	
		return -1;
	}

	private int getDistance(int cross0, int cross1) {
		/*
		 * Hinh nhu da xet 1 chieu 2 chieu
		 */
		int segmentId = getAdjacentSegmentId2(cross0, cross1);
		
		/*
		 * Xet xem co cam qua segment nay hay khong
		 * Khong xet duoc segment dau va cuoi (gan start va end nhat)
		 */
		if(bannedSegmentsIds.contains(new Integer(segmentId)))
			segmentId = -1;
		/*
		 * Xet xem segment nay co lo cot hay khong
		 * Khong xet duoc segment dau va cuoi (gan start va end nhat)
		 */
		if(LayerConfig.showConstructionSite) {
			if(MapCanvas.constructionSiteSegmentIds != null && LayerConfig.showConstructionSite) {
				if(MapCanvas.constructionSiteSegmentIds.contains(new Integer(segmentId)))
					segmentId = -1;
			}
		}
		if(segmentId != -1)
			return MapCanvas.segments[segmentId].getLength();
		return -1;
	}
	
	private int getTarget(Segment[] segments, int crossId, int segmentId) {
		if(crossId == segments[segmentId].getCross0())
			return segments[segmentId].getCross1();
		return segments[segmentId].getCross0();
	}
	
	private int[] getTargets(Crossroads[] cross, Segment[] segments, int crossId) {
		int[] targets  = new int[cross[crossId].getSegments().length];
		for(int i = 0; i < targets.length; i ++) {
			targets[i] = getTarget(segments, crossId, cross[crossId].getSegments()[i]);
		}
		return targets;
	}
	
	private int getNearestVertex() {
		int minIndex = -1;
		int minDistance = distanceArray[0];
		for(int i = 0; i < distanceArray.length; i ++) {
			if(!isVisited[i] && (distanceArray[i] != -1) && (isGreater(minDistance, distanceArray[i]))) {
				minDistance = distanceArray[i];
				minIndex = i;
			}
		}
		return minIndex;
	}

	private void initialize() {
		for(int i = 0; i < isVisited.length; i++) {
			//if(getAdjacentSegmentId(cross, source.getId(), i) == -1)
				distanceArray[i] = -1;
			//else
			//	distanceArray[i] = getDistance(cross, source.getId(), i);
			prevVertexArray[i] = -1;
			isVisited[i] = false;
		}
		segmentTrace.removeAllElements();
		crossroadsTrace.removeAllElements();
	}
	
	public boolean runDijstraAlg() {
		//System.out.println(toString());
		/*
		 * PHAI XET RIENG TRUONG HOP SOURCE VA DESTINATION NAM CUNG SEGMENT
		 * Xet 1 chieu 2 chieu
		 */
            if(sourceClosestSegmentId == destinationClosestSegmentId) {
                Segment segment = MapCanvas.segments[sourceClosestSegmentId];
                if(segment.getType2() == 1) {
                    if(destination.getX() == segment.getX(0) && destination.getY() == segment.getY(0)) {
                    	Crossroads temp = destination;
                    	destination = source;
                    	source = temp;
                    }
                }
                else {
                	//Do what???
                }
            }
            
            initialize();
		
		queue = new Queue();
		queue.enqueue(source);
		
		distanceArray[source.getId()] = 0;
		while(queue.size() > 0) {
			Crossroads current = (Crossroads)queue.dequeue();
			if(current == null)
				return false;
			if(isVisited[destination.getId()]) {
                            queue = null;
                            return true;
                        }
			
			isVisited[current.getId()] = true;
			
			for(int j = 0; j < current.getSegments().length; j++) {
				int target = getTarget(MapCanvas.segments, current.getId(), current.getSegments()[j]);
				if(!isVisited[target]) {
					int d = getDistance(current.getId(), target);
					int newDistance = distanceArray[current.getId()];
					//distanceArray[current.getId()] == -1 thi xu ly lam sao???
					
					/*
					 * Hinh nhu van con sai :-??
					 */
					if(d != -1) {
						if(newDistance != -1)
							newDistance += d;
						else
							newDistance = d;
						if(isGreater(distanceArray[target], newDistance)) {
							distanceArray[target] = newDistance;
							prevVertexArray[target] = (short)current.getId();
							queue.enqueue(MapCanvas.cross[target]);
						}
					}
				}
			}
		}
                queue = null;
		return false;
	}
	
	/*
	 * Return True a == -1
	 * Otherwise return a > b
	 */
	public boolean isGreater(int a, int b) {
		if(a == -1)
			return true;
		return (a > b);
	}

	public short[] getPrevVertexArray() {
		return prevVertexArray;
	}
	
	/*
	 * Get result
	 * 
	 */	
	public String routeToString() {
		if(source == null || destination == null)
			return "";
		
		getSegmentTrace();
		
		/*
		 * Chua xet truong hop 2 diem nam cung 1 duong
		 * 
		 * CAN XET RIENG 2 SEGMENT DAU TIEN VA CUOI CUNG
		 * 
		 * DA TINH DUOC DO DAI 2 SEGMENT DAU - CUOI
		 */
		if(crossroadsTrace.size() == 0 || segmentTrace.size() == 0)
			return "";
		
		StringBuffer route = new StringBuffer();

		for(int i = 0; i < segmentTrace.size(); i ++) {
			int id = ((Segment)segmentTrace.elementAt(i)).getPathId();
			String name = Util.decodeCharacters(MapCanvas.paths[id].getName());
			int length = ((Segment)segmentTrace.elementAt(i)).getLength();
			//System.out.println(name + " " + length + " m\n");
		}
		
		int x0, y0, x1, y1, x2, y2;
		Crossroads crossroads0 = null;
		Crossroads crossroads1 = null;
		Crossroads crossroads2 = null;
		int pathId = ((Segment)segmentTrace.elementAt(0)).getPathId();
		//int currentLength = ((Segment)segmentTrace.elementAt(0)).getLength();
		int currentLength = lengthOfSegmentPartly((Segment)segmentTrace.firstElement(), projectionSourceX, projectionSourceY, sourceClosestPointIndex, directionTowardSource);
		
		for(int i = 1; i < segmentTrace.size(); i ++) {
			Segment segment = (Segment)segmentTrace.elementAt(i);
			if(segment.getPathId() != pathId) {
				route.append(Util.decodeCharacters(MapCanvas.paths[pathId].getName()));
				route.append(" ");
				route.append(currentLength);
				route.append("m\n");
				pathId = segment.getPathId();
				currentLength = segment.getLength();
				if(i == segmentTrace.size() - 1)
					currentLength = lengthOfSegmentPartly((Segment)segmentTrace.lastElement(), projectionDestinationX, projectionDestinationY, destinationClosestPointIndex, directionTowardDestination);
				
				Segment prevSegment = (Segment)segmentTrace.elementAt(i - 1);
				/*
				 * PHAI LAM PHAN NAY VI DU LIEU KHONG CHUAN
				 * Khong biet thu tu cross 2 dau segment
				 * 
				 * KHONG NEN DUA VAO 2 DAU SEGMENT DE TINH HUONG RE
				 * NEN DUA VAO 2 DIEM KE VOI DIEM CHUNG CUA 2 SEGMENT
				 */
				if(segment.getCross0() == prevSegment.getCross0()) {
					x0 = MapCanvas.cross[prevSegment.getCross1()].getX();
					y0 = MapCanvas.cross[prevSegment.getCross1()].getY();
					x1 = MapCanvas.cross[prevSegment.getCross0()].getX();
					y1 = MapCanvas.cross[prevSegment.getCross0()].getY();
					x2 = MapCanvas.cross[segment.getCross1()].getX();
					y2 = MapCanvas.cross[segment.getCross1()].getY();
				}
				else if(segment.getCross1() == prevSegment.getCross1()) {
					x0 = MapCanvas.cross[prevSegment.getCross0()].getX();
					y0 = MapCanvas.cross[prevSegment.getCross0()].getY();
					x1 = MapCanvas.cross[prevSegment.getCross1()].getX();
					y1 = MapCanvas.cross[prevSegment.getCross1()].getY();
					x2 = MapCanvas.cross[segment.getCross0()].getX();
					y2 = MapCanvas.cross[segment.getCross0()].getY();
				}
				else if(segment.getCross0() == prevSegment.getCross1()) {
					x0 = MapCanvas.cross[prevSegment.getCross0()].getX();
					y0 = MapCanvas.cross[prevSegment.getCross0()].getY();
					x1 = MapCanvas.cross[prevSegment.getCross1()].getX();
					y1 = MapCanvas.cross[prevSegment.getCross1()].getY();
					x2 = MapCanvas.cross[segment.getCross1()].getX();
					y2 = MapCanvas.cross[segment.getCross1()].getY();
				}
				else { //(segment.getCross1() == prevSegment.getCross0()
					x0 = MapCanvas.cross[prevSegment.getCross1()].getX();
					y0 = MapCanvas.cross[prevSegment.getCross1()].getY();
					x1 = MapCanvas.cross[prevSegment.getCross0()].getX();
					y1 = MapCanvas.cross[prevSegment.getCross0()].getY();
					x2 = MapCanvas.cross[segment.getCross0()].getX();
					y2 = MapCanvas.cross[segment.getCross0()].getY();
				}
	            route.append(Util.getTurningDirection(x0, y0, x1, y1, x2, y2));
	            
	            route.append("\n");
			}
			else {
				if(i == segmentTrace.size() - 1)
					currentLength += lengthOfSegmentPartly((Segment)segmentTrace.lastElement(), projectionDestinationX, projectionDestinationY, destinationClosestPointIndex, directionTowardDestination);
				else
					currentLength += segment.getLength();
			}
		}
		route.append(Util.decodeCharacters(MapCanvas.paths[pathId].getName()));
		route.append(" ");
		route.append(currentLength);
		route.append("m");
		
		return route.toString();
	}

        public Vector getCrossroadsTrace() {
            if(crossroadsTrace.size() == 0)
                createCrossroadsTrace();
		return crossroadsTrace;
	}

	private void createCrossroadsTrace() {
		if(this.source != null && this.destination != null) {
			crossroadsTrace.removeAllElements();
			
			/*
			 * When using trace, reverse it!
			 */
			int prev = this.destination.getId();
			while (prev != -1) {
				crossroadsTrace.addElement(MapCanvas.cross[prev]);
				prev = prevVertexArray[prev];
			}
		}
		/*
		 * Do tinh toan ket qua (huong re, do dai) dua vao segment
		 * --> Co the khong can dieu chinh crossroads
		 */
		//pruneCrossroads();
		//System.out.println("CROSS BEFORE = " + crossroadsTrace.size());
		supplementCrossroads();
		
		//System.out.println("CROSS = " + crossroadsTrace.size());
	}

	/*
	 * BO SUNG THEM GIAO LO
	 * --> Sao cho diem dau va diem cuoi nam trong day giao lo
	 */
	private void supplementCrossroads() {
		if(crossroadsTrace.size() > 1) {
			Crossroads crossStart = (Crossroads)crossroadsTrace.lastElement();
			Crossroads crossNextStart = (Crossroads)crossroadsTrace.elementAt(crossroadsTrace.size() - 2);
			Crossroads crossEnd = (Crossroads)crossroadsTrace.firstElement();
			Crossroads crossPrevEnd = (Crossroads)crossroadsTrace.elementAt(1);
			
			if(getAdjacentSegmentId2(crossStart.getId(), crossNextStart.getId()) != sourceClosestSegmentId) {
				if(crossStart.getId() != MapCanvas.segments[sourceClosestSegmentId].getCross0())
					crossroadsTrace.addElement(MapCanvas.cross[MapCanvas.segments[sourceClosestSegmentId].getCross0()]);
				else
					crossroadsTrace.addElement(MapCanvas.cross[MapCanvas.segments[sourceClosestSegmentId].getCross1()]);
			}
			
			if(getAdjacentSegmentId2(crossEnd.getId(), crossPrevEnd.getId()) != destinationClosestSegmentId) {
				if(crossEnd.getId() != MapCanvas.segments[destinationClosestSegmentId].getCross0())
					crossroadsTrace.insertElementAt(MapCanvas.cross[MapCanvas.segments[destinationClosestSegmentId].getCross0()], 0);
				else
					crossroadsTrace.insertElementAt(MapCanvas.cross[MapCanvas.segments[destinationClosestSegmentId].getCross1()], 0);
			}
		}
		else if(crossroadsTrace.size() == 1) {
			Crossroads c = (Crossroads)crossroadsTrace.elementAt(0);
			if(c.getId() != MapCanvas.segments[sourceClosestSegmentId].getCross0())
				crossroadsTrace.addElement(MapCanvas.cross[MapCanvas.segments[sourceClosestSegmentId].getCross0()]);
			else
				crossroadsTrace.addElement(MapCanvas.cross[MapCanvas.segments[sourceClosestSegmentId].getCross1()]);
			if(c.getId() != MapCanvas.segments[destinationClosestSegmentId].getCross0())
				crossroadsTrace.insertElementAt(MapCanvas.cross[MapCanvas.segments[destinationClosestSegmentId].getCross0()], 0);
			else
				crossroadsTrace.insertElementAt(MapCanvas.cross[MapCanvas.segments[destinationClosestSegmentId].getCross1()], 0);
		}
		
		// For beautiful drawing 
		if(crossroadsTrace.size() > 1) {
			source = (Crossroads)crossroadsTrace.elementAt(crossroadsTrace.size() - 2);
			destination = (Crossroads)crossroadsTrace.elementAt(1);
		}
	}
	
	/*
	 * Xu ly truong hop du giao lo
	 * Tuc la truong hop diem start hoac diem end nam kep giua 2 giao lo
	 * KHONG NEN BO GIAO LO
	 */
	private void pruneCrossroads() {
		if(crossroadsTrace.size() > 1) {
			Crossroads crossStart = (Crossroads)crossroadsTrace.lastElement();
			Crossroads crossNextStart = (Crossroads)crossroadsTrace.elementAt(crossroadsTrace.size() - 2);
			Crossroads crossEnd = (Crossroads)crossroadsTrace.firstElement();
			Crossroads crossPrevEnd = (Crossroads)crossroadsTrace.elementAt(1);
			//System.out.println("CROSS = " + crossStart.getId() + " " + crossNextStart.getId());
			//System.out.println("SEG = " + sourceClosestSegmentId);
			/*
			 * 1 CHIEU ???
                         * LUU Y SAU
                         * HIEN GIO CHUA THAY LOI
			 */
			if(getAdjacentSegmentId2(crossStart.getId(), crossNextStart.getId()) == sourceClosestSegmentId) {
				source = crossNextStart;
				crossroadsTrace.removeElementAt(crossroadsTrace.size() - 1);
			}
			
			if(getAdjacentSegmentId2(crossEnd.getId(), crossPrevEnd.getId()) == destinationClosestSegmentId) {
				destination = crossPrevEnd;
				crossroadsTrace.removeElementAt(0);
			}
		}
		
	}
	
	private void createSegmentTrace() {
		if(!resultGenerated) {
			if(this.source != null && this.destination != null) {
				segmentTrace.removeAllElements();
				getCrossroadsTrace();
				for (int i = crossroadsTrace.size() - 1; i > 0; i--) {
					int id = getAdjacentSegmentId2(
							((Crossroads) crossroadsTrace.elementAt(i)).getId(),
							((Crossroads) crossroadsTrace.elementAt(i - 1)).getId());
					if(id != -1)
						segmentTrace.addElement(MapCanvas.segments[id]);
				}
	            //segmentTrace.insertElementAt(MapCanvas.segments[sourceClosestSegmentId], 0);
	            //segmentTrace.addElement(MapCanvas.segments[destinationClosestSegmentId]);

				// Tim chi so trong segment cua diem nam giua diem chieu va giao lo
				Segment segment = MapCanvas.segments[sourceClosestSegmentId];
				if(directionTowardSource == 0) {
					directionTowardSource = -1;
					//System.out.println("SOURCE " + alg.getSourceClosestPointIndex() + " "  + segment.getNumberOfPoints());;
					for(int i = sourceClosestPointIndex + 1; i < segment.getNumberOfPoints(); i ++) {
						if(segment.getX(i) == source.getX() && segment.getY(i) == source.getY()) {
							directionTowardSource = 1;
							// Chon diem gan giao lo hon
							sourceClosestPointIndex ++;
						}
					}
				}
				//Tim chi so trong segment cua diem nam giua diem chieu va giao lo
				segment = MapCanvas.segments[destinationClosestSegmentId];
				if(directionTowardDestination == 0) {
					directionTowardDestination = -1;
					//System.out.println("DEST " + alg.getDestinationClosestPointIndex() + " "  + segment.getNumberOfPoints() + " " + segment.getId());
					for(int i = destinationClosestPointIndex + 1; i < segment.getNumberOfPoints(); i ++) {
						if(segment.getX(i) == destination.getX() && segment.getY(i) == destination.getY()) {
							directionTowardDestination = 1;
							// Chon diem gan giao lo hon
							destinationClosestPointIndex ++;
						}
					}
				}
				
			}
			/*
			System.out.println("SEGMENTS = " + segmentTrace.size());
			for(int i = 0; i < segmentTrace.size(); i ++)
				System.out.println(((Segment)segmentTrace.elementAt(i)).getId() + " ");
			for(int i = 0; i < crossroadsTrace.size(); i ++)
				System.out.println(((Crossroads)crossroadsTrace.elementAt(i)).getId() + " ");
			*/
		}
	}

        public Vector getSegmentTrace() {
            if(segmentTrace.size() == 0)
                createSegmentTrace();
		return segmentTrace;
	}

	public int getSourceX() {
		return sourceX;
	}

	public void setSource(int sourceX, int sourceY) {
		/////////////////////////////////////////////
		//sourceX = 41697;
		//sourceY = 46025;
		//System.out.println("Source Projection");
		/////////////////////////////////////////////
		resultGenerated = false; // Can sinh lai ket qua
		
		this.sourceX = sourceX;
		this.sourceY = sourceY;
		
		/*
		 * 
		 */
		if(source == null) {
			listPoints.addElement(new Point(sourceX, sourceY));
		}
		else {
			listPoints.removeElementAt(0);
			listPoints.insertElementAt(new Point(sourceX, sourceY), 0);
		}
		
		// Search nearest segment and nearest straight in the nearest segment
		sourceClosestSegmentId = Util.findClosestSegment2(sourceX, sourceY);
		//System.out.println("---SEGMENT---" + segment.getId());
		
		int minPointIndex = -1;
		int minDistance = Integer.MAX_VALUE;
		for(int i = 0; i < MapCanvas.segments[sourceClosestSegmentId].getNumberOfPoints() - 1; i ++) {
			int d = (int)Util.linePointDist(new Point(MapCanvas.segments[sourceClosestSegmentId].getX(i), MapCanvas.segments[sourceClosestSegmentId].getY(i)), new Point(MapCanvas.segments[sourceClosestSegmentId].getX(i + 1), MapCanvas.segments[sourceClosestSegmentId].getY(i + 1)), new Point(sourceX, sourceY), true);
			if(minDistance > d) {
				minDistance = d;
				minPointIndex = i;
			}
		}
		
		sourceClosestPointIndex = minPointIndex;
		directionTowardSource = 0;
		//System.out.println("---SEGMENT---" + minDistance + " " + minPointIndex);
		
		// Point projection
		Point projection = Util.getProjectionPoint2Line(sourceX, sourceY, MapCanvas.segments[sourceClosestSegmentId].getX(minPointIndex), MapCanvas.segments[sourceClosestSegmentId].getY(minPointIndex), MapCanvas.segments[sourceClosestSegmentId].getX(minPointIndex + 1), MapCanvas.segments[sourceClosestSegmentId].getY(minPointIndex + 1));
		setProjectionSourceX(projection.getX());
		setProjectionSourceY(projection.getY());
		
		// Search nearest crossroadses
		// Tam xet 1 chieu 2 chieu nhu sau: xet xem segment co the di den cross nao
		Segment segment =MapCanvas.segments[sourceClosestSegmentId]; 
		if(segment.getType2() != 1) {
			int d0 = (int)Util.calculateDistancePoint2Point(getProjectionSourceX(), getProjectionSourceY(), MapCanvas.cross[segment.getCross0()].getX(), MapCanvas.cross[segment.getCross0()].getY());
			int d1 = (int)Util.calculateDistancePoint2Point(getProjectionSourceX(), getProjectionSourceY(), MapCanvas.cross[segment.getCross1()].getX(), MapCanvas.cross[segment.getCross1()].getY());
			
			if(d0 < d1)
				setSourceCrossroads(MapCanvas.cross[segment.getCross0()]);
			else
				setSourceCrossroads(MapCanvas.cross[segment.getCross1()]);
		}
		else {
			Crossroads cross0 = MapCanvas.cross[segment.getCross0()];
			Crossroads cross1 = MapCanvas.cross[segment.getCross1()];
			/*
			 * Voi doan duong 1 chieu, tu diem dau segment co the di toi diem cuoi,
			 * tu diem cuoi khong ve diem dau duoc.
			 * Nen neu cross0 la diem dau thi chon cross gan nhan la cross1,
			 * vi tu diem chieu chi di toi cross1, khong quay ve cross0 duoc.
			 * Diem chieu nam giua cross0 va cross1
			 */
			if(segment.getX(0) == cross0.getX() && segment.getY(0) == cross0.getY())
				setSourceCrossroads(cross1);
			else
				setSourceCrossroads(cross0);
		}
	}
	
	private void setSource(Point point) {
		/////////////////////////////////////////////
		//sourceX = 41697;
		//sourceY = 46025;
		//System.out.println("Source Projection");
		/////////////////////////////////////////////
		resultGenerated = false; // Can sinh lai ket qua
		
		this.sourceX = point.getX();
		this.sourceY = point.getY();
		
		// Search nearest segment and nearest straight in the nearest segment
		sourceClosestSegmentId = Util.findClosestSegment2(point.getX(), point.getY());
		//System.out.println("---SEGMENT---" + segment.getId());
		
		int minPointIndex = -1;
		int minDistance = Integer.MAX_VALUE;
		for(int i = 0; i < MapCanvas.segments[sourceClosestSegmentId].getNumberOfPoints() - 1; i ++) {
			int d = (int)Util.linePointDist(new Point(MapCanvas.segments[sourceClosestSegmentId].getX(i), MapCanvas.segments[sourceClosestSegmentId].getY(i)), new Point(MapCanvas.segments[sourceClosestSegmentId].getX(i + 1), MapCanvas.segments[sourceClosestSegmentId].getY(i + 1)), point, true);
			if(minDistance > d) {
				minDistance = d;
				minPointIndex = i;
			}
		}
		
		sourceClosestPointIndex = minPointIndex;
		directionTowardSource = 0;
		//System.out.println("---SEGMENT---" + minDistance + " " + minPointIndex);
		
		// Point projection
		Point projection = Util.getProjectionPoint2Line(sourceX, sourceY, MapCanvas.segments[sourceClosestSegmentId].getX(minPointIndex), MapCanvas.segments[sourceClosestSegmentId].getY(minPointIndex), MapCanvas.segments[sourceClosestSegmentId].getX(minPointIndex + 1), MapCanvas.segments[sourceClosestSegmentId].getY(minPointIndex + 1));
		setProjectionSourceX(projection.getX());
		setProjectionSourceY(projection.getY());
		
		// Search nearest crossroadses
		// Tam xet 1 chieu 2 chieu nhu sau: xet xem segment co the di den cross nao
		Segment segment =MapCanvas.segments[sourceClosestSegmentId]; 
		if(segment.getType2() != 1) {
			int d0 = (int)Util.calculateDistancePoint2Point(getProjectionSourceX(), getProjectionSourceY(), MapCanvas.cross[segment.getCross0()].getX(), MapCanvas.cross[segment.getCross0()].getY());
			int d1 = (int)Util.calculateDistancePoint2Point(getProjectionSourceX(), getProjectionSourceY(), MapCanvas.cross[segment.getCross1()].getX(), MapCanvas.cross[segment.getCross1()].getY());
			
			if(d0 < d1)
				setSourceCrossroads(MapCanvas.cross[segment.getCross0()]);
			else
				setSourceCrossroads(MapCanvas.cross[segment.getCross1()]);
		}
		else {
			Crossroads cross0 = MapCanvas.cross[segment.getCross0()];
			Crossroads cross1 = MapCanvas.cross[segment.getCross1()];
			/*
			 * Voi doan duong 1 chieu, tu diem dau segment co the di toi diem cuoi,
			 * tu diem cuoi khong ve diem dau duoc.
			 * Nen neu cross0 la diem dau thi chon cross gan nhan la cross1,
			 * vi tu diem chieu chi di toi cross1, khong quay ve cross0 duoc.
			 * Diem chieu nam giua cross0 va cross1
			 */
			if(segment.getX(0) == cross0.getX() && segment.getY(0) == cross0.getY())
				setSourceCrossroads(cross1);
			else
				setSourceCrossroads(cross0);
		}
	}

	public int getDestinationX() {
		return destinationX;
	}

	public void setDestination(int destinationX, int destinationY) {
		///////////////////////////////////////
		//this.destinationX = 41622;
		//this.destinationY = 46118;
		//System.out.println("Destination Projection");
		///////////////////////////////////////
		resultGenerated = false; // Can sinh lai ket qua
		
		this.destinationX = destinationX;
		this.destinationY = destinationY;

		/*
		 * So luong diem khong the bang 0
		 * Do phai co diem source
		 */
		if(destination == null) {
			// chua co diem dich
			listPoints.addElement(new Point(destinationX, destinationY));
		}
		else { 
			// thay diem dich moi
			listPoints.removeElementAt(listPoints.size() - 1);
			listPoints.addElement(new Point(destinationX, destinationY));
		}
			
		// Search nearest segment and nearest straight in the nearest segment
		destinationClosestSegmentId = Util.findClosestSegment2(destinationX, destinationY);
		int minPointIndex = -1;
		int minDistance = Integer.MAX_VALUE;
		for(int i = 0; i < MapCanvas.segments[destinationClosestSegmentId].getNumberOfPoints() - 1; i ++) {
			int d = (int)Util.linePointDist(new Point(MapCanvas.segments[destinationClosestSegmentId].getX(i), MapCanvas.segments[destinationClosestSegmentId].getY(i)), new Point(MapCanvas.segments[destinationClosestSegmentId].getX(i + 1), MapCanvas.segments[destinationClosestSegmentId].getY(i + 1)), new Point(destinationX, destinationY), true);
			if(minDistance > d) {
				minDistance = d;
				minPointIndex = i;
			}
		}
		destinationClosestPointIndex = minPointIndex;
		directionTowardDestination = 0;
		// Point projection
		Point projection = Util.getProjectionPoint2Line(destinationX, destinationY, MapCanvas.segments[destinationClosestSegmentId].getX(minPointIndex), MapCanvas.segments[destinationClosestSegmentId].getY(minPointIndex), MapCanvas.segments[destinationClosestSegmentId].getX(minPointIndex + 1), MapCanvas.segments[destinationClosestSegmentId].getY(minPointIndex + 1));
		setProjectionDestinationX(projection.getX());
		setProjectionDestinationY(projection.getY());
		
		// Search nearest crossroadses
		// Tam xet 1 chieu 2 chieu nhu sau: xet xem segment co the di den cross nao
		Segment segment =MapCanvas.segments[destinationClosestSegmentId]; 
		if(segment.getType2() != 1) {
			int d0 = (int)Util.calculateDistancePoint2Point(getProjectionDestinationX(), getProjectionDestinationY(), MapCanvas.cross[segment.getCross0()].getX(), MapCanvas.cross[segment.getCross0()].getY());
			int d1 = (int)Util.calculateDistancePoint2Point(getProjectionDestinationX(), getProjectionDestinationY(), MapCanvas.cross[segment.getCross1()].getX(), MapCanvas.cross[segment.getCross1()].getY());
			
			if(d0 < d1)
				setDestinationCrossroads(MapCanvas.cross[segment.getCross0()]);
			else
				setDestinationCrossroads(MapCanvas.cross[segment.getCross1()]);
		}
		else {
			Crossroads cross0 = MapCanvas.cross[segment.getCross0()];
			Crossroads cross1 = MapCanvas.cross[segment.getCross1()];
			/*
			 * VOI SOURCE:
			 * Voi doan duong 1 chieu, tu diem dau segment co the di toi diem cuoi,
			 * tu diem cuoi khong ve diem dau duoc.
			 * Nen neu cross0 la diem dau thi chon cross gan nhan la cross1,
			 * vi tu diem chieu chi di toi cross1, khong quay ve cross0 duoc.
			 * Diem chieu nam giua cross0 va cross1
			 * 
			 * VOI DESTINATION:
			 * <NGUOC VOI SOURCE>
			 * SRC: chon giao lo ma source co the di toi
			 * DEST: chon giao lo co the di toi destination 
			 */
			if(segment.getX(0) == cross0.getX() && segment.getY(0) == cross0.getY())
				setDestinationCrossroads(cross0);
			else
				setDestinationCrossroads(cross1);
		}
	}

	private void setDestination(Point point) {
		///////////////////////////////////////
		//this.destinationX = 41622;
		//this.destinationY = 46118;
		//System.out.println("Destination Projection");
		///////////////////////////////////////
		resultGenerated = false; // Can sinh lai ket qua
		
		this.destinationX = point.getX();
		this.destinationY = point.getY();

		// Search nearest segment and nearest straight in the nearest segment
		destinationClosestSegmentId = Util.findClosestSegment2(destinationX, destinationY);
		int minPointIndex = -1;
		int minDistance = Integer.MAX_VALUE;
		for(int i = 0; i < MapCanvas.segments[destinationClosestSegmentId].getNumberOfPoints() - 1; i ++) {
			int d = (int)Util.linePointDist(new Point(MapCanvas.segments[destinationClosestSegmentId].getX(i), MapCanvas.segments[destinationClosestSegmentId].getY(i)), new Point(MapCanvas.segments[destinationClosestSegmentId].getX(i + 1), MapCanvas.segments[destinationClosestSegmentId].getY(i + 1)), new Point(destinationX, destinationY), true);
			if(minDistance > d) {
				minDistance = d;
				minPointIndex = i;
			}
		}
		destinationClosestPointIndex = minPointIndex;
		directionTowardDestination = 0;
		// Point projection
		Point projection = Util.getProjectionPoint2Line(destinationX, destinationY, MapCanvas.segments[destinationClosestSegmentId].getX(minPointIndex), MapCanvas.segments[destinationClosestSegmentId].getY(minPointIndex), MapCanvas.segments[destinationClosestSegmentId].getX(minPointIndex + 1), MapCanvas.segments[destinationClosestSegmentId].getY(minPointIndex + 1));
		setProjectionDestinationX(projection.getX());
		setProjectionDestinationY(projection.getY());
		
		// Search nearest crossroadses
		// Tam xet 1 chieu 2 chieu nhu sau: xet xem segment co the di den cross nao
		Segment segment =MapCanvas.segments[destinationClosestSegmentId]; 
		if(segment.getType2() != 1) {
			int d0 = (int)Util.calculateDistancePoint2Point(getProjectionDestinationX(), getProjectionDestinationY(), MapCanvas.cross[segment.getCross0()].getX(), MapCanvas.cross[segment.getCross0()].getY());
			int d1 = (int)Util.calculateDistancePoint2Point(getProjectionDestinationX(), getProjectionDestinationY(), MapCanvas.cross[segment.getCross1()].getX(), MapCanvas.cross[segment.getCross1()].getY());
			
			if(d0 < d1)
				setDestinationCrossroads(MapCanvas.cross[segment.getCross0()]);
			else
				setDestinationCrossroads(MapCanvas.cross[segment.getCross1()]);
		}
		else {
			Crossroads cross0 = MapCanvas.cross[segment.getCross0()];
			Crossroads cross1 = MapCanvas.cross[segment.getCross1()];
			/*
			 * VOI SOURCE:
			 * Voi doan duong 1 chieu, tu diem dau segment co the di toi diem cuoi,
			 * tu diem cuoi khong ve diem dau duoc.
			 * Nen neu cross0 la diem dau thi chon cross gan nhan la cross1,
			 * vi tu diem chieu chi di toi cross1, khong quay ve cross0 duoc.
			 * Diem chieu nam giua cross0 va cross1
			 * 
			 * VOI DESTINATION:
			 * <NGUOC VOI SOURCE>
			 * SRC: chon giao lo ma source co the di toi
			 * DEST: chon giao lo co the di toi destination 
			 */
			if(segment.getX(0) == cross0.getX() && segment.getY(0) == cross0.getY())
				setDestinationCrossroads(cross0);
			else
				setDestinationCrossroads(cross1);
		}
	}
	
	public int getDestinationY() {
		return destinationY;
	}

	public void setDestinationY(int destinationY) {
		this.destinationY = destinationY;
	}

	public int getSourceY() {
		return sourceY;
	}

	public void setSourceY(int sourceY) {
		this.sourceY = sourceY;
	}

	public int getProjectionSourceX() {
		return projectionSourceX;
	}

	public void setProjectionSourceX(int projectionSourceX) {
		this.projectionSourceX = projectionSourceX;
	}

	public int getProjectionSourceY() {
		return projectionSourceY;
	}

	public void setProjectionSourceY(int projectionSourceY) {
		this.projectionSourceY = projectionSourceY;
	}

	public int getProjectionDestinationX() {
		return projectionDestinationX;
	}

	public void setProjectionDestinationX(int projectionDestinationX) {
		this.projectionDestinationX = projectionDestinationX;
	}

	public int getProjectionDestinationY() {
		return projectionDestinationY;
	}

	public void setProjectionDestinationY(int projectionDestinationY) {
		this.projectionDestinationY = projectionDestinationY;
	}

	public Crossroads getSourceCrossroads() {
		return source;
	}

	public void setSourceCrossroads(Crossroads source) {
		this.source = source;
	}

	public Crossroads getDestinationCrossroads() {
		return destination;
	}

	public void setDestinationCrossroads(Crossroads destination) {
		this.destination = destination;
	}

	public int[] getDistanceArray() {
		return distanceArray;
	}
	
	public int getTotalLength() {
		int length = 0;
		for(int i = 1; i < segmentTrace.size() - 1; i ++) {
			length += ((Segment)segmentTrace.elementAt(i)).getLength();
		}
		length += lengthOfSegmentPartly((Segment)segmentTrace.firstElement(), projectionSourceX, projectionSourceY, sourceClosestPointIndex, directionTowardSource);
		length += lengthOfSegmentPartly((Segment)segmentTrace.lastElement(), projectionDestinationX, projectionDestinationY, destinationClosestPointIndex, directionTowardDestination);
		return length;
	}

	public int getSourceClosestSegmentId() {
		return sourceClosestSegmentId;
	}

	public int getDestinationClosestSegmentId() {
		return destinationClosestSegmentId;
	}

	public int getSourceClosestPointIndex() {
		return sourceClosestPointIndex;
	}

	public int getDestinationClosestPointIndex() {
		return destinationClosestPointIndex;
	}

	public void setSourceClosestPointIndex(int sourceClosestPointIndex) {
		this.sourceClosestPointIndex = sourceClosestPointIndex;
	}

	public void setDestinationClosestPointIndex(int destinationClosestPointIndex) {
		this.destinationClosestPointIndex = destinationClosestPointIndex;
	}

	public int getDirectionTowardSource() {
		return directionTowardSource;
	}

	public void setDirectionTowardSource(int directionTowardSource) {
		this.directionTowardSource = directionTowardSource;
	}

	public int getDirectionTowardDestination() {
		return directionTowardDestination;
	}

	public void setDirectionTowardDestination(int directionTowardDestination) {
		this.directionTowardDestination = directionTowardDestination;
	}
	
	public String toString() {
		StringBuffer strBuf = new StringBuffer();
		
		strBuf.append("ALGORITHM INFO\n");
		
		strBuf.append("Source = " + sourceX + " " + sourceY + "\n");
		strBuf.append("Destination = " + destinationX + " " + destinationY + "\n");
		
		strBuf.append("ProjectionSource = " + projectionSourceX + " " + projectionSourceY + "\n");
		strBuf.append("ProjectionDestination = " + projectionDestinationX + " " + projectionDestinationY + "\n");
		
		strBuf.append("SourceClosestSegmentId = " + sourceClosestSegmentId + "\n");
		strBuf.append("SourceClosestPointIndex = " + sourceClosestPointIndex + "\n");
		
		strBuf.append("DestinationClosestSegmentId = " + destinationClosestSegmentId + "\n");
		strBuf.append("DestinationClosestPointIndex = " + destinationClosestPointIndex + "\n");
		
		strBuf.append("SourceCrossId = " + source.getId() + "\n");
		strBuf.append("DestinationCrossId = " + destination.getId() + "\n");
		
		//strBuf.append("NumOfSegments = " + segmentTrace.size() + "\n");
		//strBuf.append("NumOfCrossroadses = " + crossroadsTrace.size() + "\n");
		
		return strBuf.toString();
	}

	public boolean isResultGenerated() {
		return resultGenerated;
	}
	
	private int lengthOfSegmentPartly(Segment segment, int projX, int projY, int closestPointIndex, int dir) {
		/*
		 * CO THE CONG THIEU
		 * CON DOAN TU PROJ_SOURCE DEN DIEM GAN NHAT ???
		 */
		int length = 0;
		length += Util.calculateDistancePoint2Point(projX, projY, segment.getX(closestPointIndex), segment.getY(closestPointIndex));
		if(dir == 1) {
			for(int i = closestPointIndex; i < segment.getNumberOfPoints() - 1; i ++)
				length += Util.calculateDistancePoint2Point(segment.getX(i), segment.getY(i), segment.getX(i + 1), segment.getY(i + 1));
		}
		else if(dir == -1) {
			for(int i = closestPointIndex; i > 0; i --)
				length += Util.calculateDistancePoint2Point(segment.getX(i), segment.getY(i), segment.getX(i - 1), segment.getY(i - 1));
		}
		return length;
	}
	
	public String routeToSoundString() {
		if(source == null || destination == null)
			return "";
		
		getSegmentTrace();
		
		/*
		 * Chua xet truong hop 2 diem nam cung 1 duong
		 * 
		 * CAN XET RIENG 2 SEGMENT DAU TIEN VA CUOI CUNG
		 * 
		 * DA TINH DUOC DO DAI 2 SEGMENT DAU - CUOI
		 */
		if(crossroadsTrace.size() == 0 || segmentTrace.size() == 0)
			return "";
		
		StringBuffer route = new StringBuffer();

		for(int i = 0; i < segmentTrace.size(); i ++) {
			int id = ((Segment)segmentTrace.elementAt(i)).getPathId();
			String name = Util.decodeCharacters(MapCanvas.paths[id].getName());
			int length = ((Segment)segmentTrace.elementAt(i)).getLength();
			//System.out.println(name + " " + length + " m\n");
		}
		
		int x0, y0, x1, y1, x2, y2;
		Crossroads crossroads0 = null;
		Crossroads crossroads1 = null;
		Crossroads crossroads2 = null;
		int pathId = ((Segment)segmentTrace.elementAt(0)).getPathId();
		//int currentLength = ((Segment)segmentTrace.elementAt(0)).getLength();
		int currentLength = lengthOfSegmentPartly((Segment)segmentTrace.firstElement(), projectionSourceX, projectionSourceY, sourceClosestPointIndex, directionTowardSource);
		
		for(int i = 1; i < segmentTrace.size(); i ++) {
			Segment segment = (Segment)segmentTrace.elementAt(i);
			if(segment.getPathId() != pathId) {
				//route.append(pathId);
				route.append(Util.decodeCharacters(MapCanvas.paths[pathId].getName()));
				//route.append(" ");
				//route.append(currentLength);
				//route.append("m\n");
				route.append("\n");
				pathId = segment.getPathId();
				currentLength = segment.getLength();
				if(i == segmentTrace.size() - 1)
					currentLength = lengthOfSegmentPartly((Segment)segmentTrace.lastElement(), projectionDestinationX, projectionDestinationY, destinationClosestPointIndex, directionTowardDestination);
				
				Segment prevSegment = (Segment)segmentTrace.elementAt(i - 1);
				/*
				 * PHAI LAM PHAN NAY VI DU LIEU KHONG CHUAN
				 * Khong biet thu tu cross 2 dau segment
				 * 
				 * KHONG NEN DUA VAO 2 DAU SEGMENT DE TINH HUONG RE
				 * NEN DUA VAO 2 DIEM KE VOI DIEM CHUNG CUA 2 SEGMENT
				 */
				if(segment.getCross0() == prevSegment.getCross0()) {
					x0 = MapCanvas.cross[prevSegment.getCross1()].getX();
					y0 = MapCanvas.cross[prevSegment.getCross1()].getY();
					x1 = MapCanvas.cross[prevSegment.getCross0()].getX();
					y1 = MapCanvas.cross[prevSegment.getCross0()].getY();
					x2 = MapCanvas.cross[segment.getCross1()].getX();
					y2 = MapCanvas.cross[segment.getCross1()].getY();
				}
				else if(segment.getCross1() == prevSegment.getCross1()) {
					x0 = MapCanvas.cross[prevSegment.getCross0()].getX();
					y0 = MapCanvas.cross[prevSegment.getCross0()].getY();
					x1 = MapCanvas.cross[prevSegment.getCross1()].getX();
					y1 = MapCanvas.cross[prevSegment.getCross1()].getY();
					x2 = MapCanvas.cross[segment.getCross0()].getX();
					y2 = MapCanvas.cross[segment.getCross0()].getY();
				}
				else if(segment.getCross0() == prevSegment.getCross1()) {
					x0 = MapCanvas.cross[prevSegment.getCross0()].getX();
					y0 = MapCanvas.cross[prevSegment.getCross0()].getY();
					x1 = MapCanvas.cross[prevSegment.getCross1()].getX();
					y1 = MapCanvas.cross[prevSegment.getCross1()].getY();
					x2 = MapCanvas.cross[segment.getCross1()].getX();
					y2 = MapCanvas.cross[segment.getCross1()].getY();
				}
				else { //(segment.getCross1() == prevSegment.getCross0()
					x0 = MapCanvas.cross[prevSegment.getCross1()].getX();
					y0 = MapCanvas.cross[prevSegment.getCross1()].getY();
					x1 = MapCanvas.cross[prevSegment.getCross0()].getX();
					y1 = MapCanvas.cross[prevSegment.getCross0()].getY();
					x2 = MapCanvas.cross[segment.getCross0()].getX();
					y2 = MapCanvas.cross[segment.getCross0()].getY();
				}
	            route.append(Util.getTurningDirection(x0, y0, x1, y1, x2, y2));
	            
	            route.append("\n");
			}
			else {
				if(i == segmentTrace.size() - 1)
					currentLength += lengthOfSegmentPartly((Segment)segmentTrace.lastElement(), projectionDestinationX, projectionDestinationY, destinationClosestPointIndex, directionTowardDestination);
				else
					currentLength += segment.getLength();
			}
		}
		route.append(Util.decodeCharacters(MapCanvas.paths[pathId].getName()));
		//route.append(" ");
		//route.append(currentLength);
		//route.append("m");
		
		return route.toString();
	}
	
	/*
	 * Hamiltonian path
	 */
	private Vector listPoints;

        public int getNumberOfPoints() {
            return listPoints.size();
        }

        public Point getPoint(int index) {
            return (Point)listPoints.elementAt(index);
        }

	public void addIntermediate(int x, int y) {
		//listPoints.addElement(new Point(x, y));
		//System.out.println("Add point " + (listPoints.size() - 1) + " : " + x + " " + y);
		if(destination == null)
			listPoints.addElement(new Point(x, y));
		else {
			listPoints.insertElementAt(new Point(x, y), listPoints.size() - 1);
		}
	}

        public void removeIntermediate(int x, int y) {
            for(int i = 1; i < listPoints.size() - 1; i ++) {
                Point point  = (Point)listPoints.elementAt(i);
                if(Math.abs(point.getX() - x) < 5 && Math.abs(point.getY() - y) < 5) {
                    listPoints.removeElementAt(i);
                }
            }
        }

        public void removeIntermediate(int index) {
            listPoints.removeElementAt(index);
        }
        public void insertIntermediate(int x, int y, int index) {
        	listPoints.insertElementAt(new Point(x, y), index);
        }
	public void findHamiltonianPath() {
		//TODO: implement finding Hamiltonian path
		//segmentTrace.addElement(MapCanvas.segments[sourceClosestSegmentId]);
		//segmentTrace.addElement(MapCanvas.segments[destinationClosestSegmentId]);
		Vector segments = new Vector();
		for(int i = 0; i < listPoints.size() - 1; i ++) {
			setSource((Point)listPoints.elementAt(i));
			setDestination((Point)listPoints.elementAt(i + 1));
			if(runDijstraAlg()) {
				
			}
			createSegmentTrace();
			for(int j = 0; j < segmentTrace.size(); j ++) {
				segments.addElement(segmentTrace.elementAt(j));
			}
		}
		segmentTrace = segments;
		setSource((Point)listPoints.firstElement());
		setDestination((Point)listPoints.lastElement());
        //System.out.println("NUMPOINTS = " + listPoints.size());
	}
}
